#include "synblepbuffer.h"

#include <math.h>

#define PI 3.14159265358979f
#define LERP(A,B,F) ((B - A) * F + A)

namespace Synth {

const int zeroCrossings = 16, overSampling = BLEP_DS_FACTOR;
const int kBlepTableSize = (zeroCrossings * 2 * overSampling);
const int n = kBlepTableSize + 1;
float blep_table[n];
float buffer1[n];
float buffer2[n];
float realTime[n];
float imagTime[n];
float realFreq[n];
float imagFreq[n];

// Generate Blackman Window
void BlackmanWindow(float *w) {
	const int m = n - 1;
	float fm = (float)m;
	for(int i = 0; i <= m; i++)	{
		float f1 = (2.0f * PI * (float)i) / fm;
		float f2 = 2.0f * f1;
		w[i] = 0.42f - (0.5f * cosf(f1)) + (0.08f * cosf(f2));
	}
}

// TODO: Merge the FTs
// Discrete Fourier Transform
void gDFT(float *realIn, float *imagIn, float *realOut, float *imagOut, float scale, float sign) {
	for (int k = 0; k < n; k++) {
		realOut[k] = 0.0f;
		imagOut[k] = 0.0f;
	}
	for (int k = 0; k < n; k++) {
		for (int i = 0; i < n; i++)	{
			float p = (2.0f * PI * (float)(k * i)) / n;
			float sr = cosf(p);
			float si = -sinf(p);
			realOut[k] += (realIn[i] * sr) - (imagIn[i] * si) * sign;
			imagOut[k] += (realIn[i] * si) + (imagIn[i] * sr) * sign;
		}
		realOut[k] *= scale;
		imagOut[k] *= scale;
	}
}

#define DFT(b, c, d, e) gDFT(b, c, d, e, 1, 1)
#define InverseDFT(b, c, d, e) gDFT(b, c, d, e, 1.0f / n, -1)

// Complex Absolute Value
inline float cabs(float x, float y) {
	return sqrtf((x * x) + (y * y));
}

// Complex Exponential
inline void cexp(float x, float y, float *zx, float *zy) {
	float expx = expf(x);
	*zx = expx * cosf(y);
	*zy = expx * sinf(y);
}

// Compute Real Cepstrum Of Signal
void RealCepstrum(float *signal, float *realCepstrum) {
	// Compose Complex FFT Input
	for (int i = 0; i < n; i++) {
		realTime[i] = signal[i];
		imagTime[i] = 0.0f;
	}
	// Perform DFT
	DFT(realTime, imagTime, realFreq, imagFreq);
    // Calculate Log Of Absolute Value
	for (int i = 0; i < n; i++) {
		realFreq[i] = logf(cabs(realFreq[i], imagFreq[i]));
		imagFreq[i] = 0.0f;
	}
	// Perform Inverse FFT
	InverseDFT(realFreq, imagFreq, realTime, imagTime);
	// Output Real Part Of FFT
	for (int i = 0; i < n; i++)
		realCepstrum[i] = realTime[i];
}

// Compute Minimum Phase Reconstruction Of Signal
void MinimumPhase(float *realCepstrum, float *minimumPhase) {
	int nd2 = n / 2;
	//if (n & 1) {
		realTime[0] = realCepstrum[0];
		for (int i = 1; i < nd2; i++)
			realTime[i] = 2.0f * realCepstrum[i];
		for (int i = nd2; i < n; i++)
			realTime[i] = 0.0f;
/*	} else {
		realTime[0] = realCepstrum[0];
		for (int i = 1; i < nd2; i++)
			realTime[i] = 2.0f * realCepstrum[i];
		realTime[nd2] = realCepstrum[nd2];
		for (int i = nd2 + 1; i < n; i++)
			realTime[i] = 0.0f; */
	//}
	for (int i = 0; i < n; i++)
		imagTime[i] = 0.0f;
	DFT(realTime, imagTime, realFreq, imagFreq);
	for (int i = 0; i < n; i++)
		cexp(realFreq[i], imagFreq[i], &realFreq[i], &imagFreq[i]);
	InverseDFT(realFreq, imagFreq, realTime, imagTime);
	for (int i = 0; i < n; i++)
		minimumPhase[i] = realTime[i];
}

// Sinc Function
inline float Sinc(float x) {
	if (x == 0.0f)
		return 1.0f;
	else {
		float pix = PI * x;
		return sinf(pix) / pix;
	}
}

// Generate MinBLEP And Return It In An Array Of Floating Point Values
void GenerateMinBLEP(float *minBLEP) {
	// Generate Sinc
	float a = (float)-zeroCrossings;
	float b = (float)zeroCrossings;
	for (int i = 0; i < n; i++) {
		float r = ((float)i) / ((float)(n - 1));
		buffer1[i] = Sinc(a + (r * (b - a)));
	}

	// Window Sinc
	BlackmanWindow(buffer2);
	for (int i = 0; i < n; i++)
		buffer1[i] *= buffer2[i];

	// Minimum Phase Reconstruction
	RealCepstrum(buffer1, buffer2);
	MinimumPhase(buffer2, buffer1);

	// Integrate Into MinBLEP
	float acc = 0.0f;
	for (int i = 0; i < n; i++) {
		acc += buffer1[i];
		minBLEP[i] = acc;
	}

	// Normalize
	float norm = 1.0f / minBLEP[n - 1];
	for (int i = 0; i < n; i++)
		minBLEP[i] *= norm;
}

void BlepInit() {
	// load table
	GenerateMinBLEP(blep_table);
}


BlepBuffer::BlepBuffer() {
	// buffer
	buffer_size = kBlepTableSize / BLEP_DS_FACTOR;
	buffer = new double[buffer_size];
	buffer_pos = 0;
	nInit = 0;
}

// add impulse into buffer
void BlepBuffer::AddBlep(float offset, float amp) {
	double *lpOut = buffer + buffer_pos;
	float *lpIn = blep_table + (int)(BLEP_DS_FACTOR * offset);
	double frac = fmodf(BLEP_DS_FACTOR * offset, 1.0);
	int cBLEP = (kBlepTableSize / BLEP_DS_FACTOR) - 1;
	double *lpBufferEnd = buffer + buffer_size;

	int i;
	// add
	for (i = 0; i < nInit; i++) {
		if (lpOut >= lpBufferEnd)
			lpOut = buffer;
		double f = LERP(lpIn[0], lpIn[1], frac);
		*lpOut += amp * (1 - f);
		lpIn += BLEP_DS_FACTOR;
		lpOut++;
	}

	// copy
	for (; i < cBLEP; i++) {
		if (lpOut >= lpBufferEnd)
			lpOut = buffer;
		double f = LERP(lpIn[0], lpIn[1], frac);
		*lpOut = amp * (1 - f);
		lpIn += BLEP_DS_FACTOR;
		lpOut++;
	}
	nInit = cBLEP;
}

float BlepBuffer::Advance() {
	float value = 0.0f;
	if (nInit) {
		value = (float)buffer[buffer_pos];
		nInit--;
		if (++buffer_pos >= buffer_size) // loop ring buffer
			buffer_pos = 0;
	}
	return value;
}

}  // namespace